#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/io.h>
#include <typeinfo>
#include <math.h>
#include <pcl/console/time.h>
using namespace std;

double distance(vector<float> normal)
{
    return abs(normal[3])/sqrt(pow(normal[0],2)+pow(normal[1],2)+pow(normal[2],2));
}

int main(int argc,char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filter_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ExtractIndices<pcl::PointXYZ> extract;               // 创建提取对象专门用于提取indices
    pcl::console::TicToc tc;
    tc.tic();
    int k = 0;
    double mindistance;
    if(argv[1] == NULL)
    {
        pcl::io::loadPCDFile("../点云/3.pcd",*cloud);
    }
    else
    {
        pcl::io::loadPCDFile(argv[1],*cloud);
    }
    if(cloud->points.size() == 0)
    {
        cerr<<"打开失败"<<endl;
        return 0;
    }

    //1.去除nan点
    std::vector<int> mapping;
    pcl::removeNaNFromPointCloud(*cloud, *filter_cloud, mapping);
    cout<<filter_cloud->points.size()<<endl;

    //2.下采样滤波,减少数据量
    pcl::PointCloud<pcl::PointXYZ>::Ptr vg_after_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(filter_cloud);
    vg.setLeafSize(0.0055f,0.0055f,0.0055f);    //设置体素栅格大小
    vg.filter(*filter_cloud);    // 下采样
    cout<<"下采样滤波之后的点个数:"<<filter_cloud->points.size()<<endl;

    //3.保留指定范围的点,超出边界的过滤掉
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(filter_cloud);
    pass.setFilterFieldName("y");              // 绿色轴
    pass.setFilterLimits(-1.8, -0.03);
    pass.filter(*filter_cloud);
    cout<<"坐标筛选之后点个数为"<<filter_cloud->points.size()<<endl;

    //4.创建kdtree的搜索方法
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
    kdtree->setInputCloud (filter_cloud);

    //5.欧式分割算法,将模型分解成一片片的聚类
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;   //Euclidean聚类对象
    clustering.setClusterTolerance(0.015);    //设置聚类的最小值为2cm
    clustering.setMinClusterSize(20);
    clustering.setMaxClusterSize(3000);
    clustering.setSearchMethod(kdtree);
    clustering.setInputCloud(filter_cloud);
    std::vector<pcl::PointIndices> clusters;
    clustering.extract(clusters);    //存储分割出来的聚类

    //6.分别对每个聚类按照索引提取子集
    pcl::PointIndices::Ptr temp (new pcl::PointIndices);  // 用于从vector中取出某个Indices并用智能指针来操作
    pcl::PointIndices::Ptr temp1 (new pcl::PointIndices);
    pcl::PointIndices::Ptr temp2 (new pcl::PointIndices);

    std::vector<pcl::ModelCoefficients::Ptr> coefficientsline ;

    for(int i = 0; i < clusters.size(); i++)
    {
        temp->indices = clusters[i].indices;
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud (new pcl::PointCloud<pcl::PointXYZ>);  //用来存储筛选出来的子集

        //6.1.分别提取处分割出来的每一个聚类
        extract.setNegative(false); // 筛选不属于平面模型的点，如果为true
        extract.setInputCloud(filter_cloud);
        extract.setIndices(temp);
        extract.filter(*temp_cloud);

        //6.2.创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers.用于记录结果
        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
        pcl::SACSegmentation<pcl::PointXYZ> seg;     //创建分个对象
        seg.setInputCloud(temp_cloud);
        seg.setOptimizeCoefficients(true);    //可选择的配置,设置模型系数需要优化
        seg.setModelType(pcl::SACMODEL_PLANE);    //必要的配置,设置分割的模型类型,所用的随机参数估计方法,距离阈值,输入点云
        seg.setMethodType(pcl::SAC_RANSAC);    //设置随机采样一致性方法类型
        seg.setMaxIterations(100);         //在放弃之前,设置最大迭代次数
        seg.setDistanceThreshold(0.01);   //设定距离阈值,距离阈值决定了点被认为是局内点时必须满足的条件
        //cout<<1<<endl;
        seg.segment(*inliers,*coefficients);   //引发分割实现,存储分割结果到点集合inliers及存储平面模型的系数coefficients

        //6.3.根据平面索引来提取子集
        extract.setNegative(false); // 筛选不属于平面模型的点，如果为true
        extract.setInputCloud(temp_cloud);
        extract.setIndices(inliers);
        extract.filter(*temp_cloud);

        if(temp_cloud->points.size() != 0)    
        {
            coefficientsline.push_back(coefficients);
        }
        else
        {
            continue;
        }
        /*temp1->indices = clusters[i+1].indices;
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud1 (new pcl::PointCloud<pcl::PointXYZ>);  //用来存储筛选出来的子集

        extract.setNegative(false);
        extract.setInputCloud(filter_cloud);
        extract.setIndices(temp1);
        extract.filter(*temp_cloud1);

        pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients ());
        std::vector<pcl::ModelCoefficients::Ptr> coefficientsline;
        pcl::PointIndices::Ptr inliers1 (new pcl::PointIndices ());
        seg.setInputCloud(temp_cloud1);
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE); 
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setMaxIterations(100);
        seg.setDistanceThreshold(0.01); 
        seg.segment(*inliers1,*coefficients1);

        extract.setNegative(false);
        extract.setInputCloud(temp_cloud1);
        extract.setIndices(inliers1);
        extract.filter(*temp_cloud1);

        //6.4.筛选出满足条件的indiced
        if(i == 0)
        {
            mindistance = distance(coefficients->values);
        }
        if(mindistance >= distance(coefficients1->values))
        {
            mindistance = distance(coefficients1->values);
            k = i+1;
        }
        //temp2->indices = clusters[k].indices;*/
    }

    for(int i = 0;i < coefficientsline.size();i++)
    {
        int j = i+1;
        if(j == coefficientsline.size())   break;
        if(i == 0)
        {
            mindistance = distance(coefficientsline[i]->values);
        }
        //cout<<distance(coefficientsline[j]->values)<<endl;
        if(distance(coefficientsline[j]->values) < mindistance)
        {
            mindistance = distance(coefficientsline[j]->values);
            k = j;
        }
        //temp2->indices = clusters[k].indices;
    }

    cout<<"坐标原点距离最近弹药箱的距离"<<mindistance<<"m"<<endl;

    //7.获得弹药箱indices,提取点云
    temp2->indices = clusters[k].indices;
    extract.setNegative(false);
    extract.setInputCloud(filter_cloud);
    extract.setIndices(temp2);
    extract.filter(*filter_cloud);

    //8.计算出提取弹药箱中点坐标
    pcl::PointXYZ min;
    pcl::PointXYZ max;
    pcl::PointXYZ center;
    pcl::getMinMax3D(*filter_cloud,min,max);
    // cout<<min<<" "<<max<<endl;
    center.x = (min.x + max.x)/2;
    center.y = (min.y + max.y)/2;
    center.z = (min.z + max.z)/2;
    cout<<"中点坐标:["<<center.x<<","<<center.y<<","<<center.z<<"]"<<endl;
    cout<<"使用时间:"<<tc.toc()<<"ms"<<endl;
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(filter_cloud);
    char b = getchar();  // 产生中断，从而保持视窗打开

    return 0;
}
